
// Copyright (c) 2012, 2013, 2014 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <cstdlib>

#include "openMVG/sfm/pipelines/global/sfm_global_engine_relative_motions.hpp"
#include "openMVG/system/timer.hpp"
#include "openMVG/cameras/Cameras_Common_command_line_helper.hpp"
#include "openMVG/cameras/Camera_Pinhole.hpp"
#include "openMVG/cameras/Camera_Pinhole_Radial.hpp"
#include "openMVG/sfm/sfm_data_io.hpp"

using namespace openMVG;
using namespace openMVG::sfm;

#include "third_party/cmdLine/cmdLine.h"
#include "third_party/stlplus3/filesystemSimplified/file_system.hpp"

std::string sSfM_Data_Filename{ "" };
std::string sDataRootDir = "D:/experiment/Dubrovnik6K/";
std::string sBundlerOutFile = "bundle/bundle.orig.out";
std::string sImgNameList = "list.orig.size.txt";
std::string sOutDir = "D:/experiment/Dubrovnik/";

int main(int argc, char **argv)
{
	using namespace std;

	// Load input SfM_Data scene
	SfM_Data sfm_data;
	if (!Load(sfm_data, sSfM_Data_Filename, ESfM_Data(VIEWS | INTRINSICS | EXTRINSICS))) {
		std::cerr << std::endl
		<< "The input SfM_Data file \"" << sSfM_Data_Filename << "\" cannot be read." << std::endl;
		return EXIT_FAILURE;
	}
	sfm_data.s_root_path = sDataRootDir;

	std::vector<std::string> img_names;
	img_names.reserve(8000);
	std::vector<int> img_height_width;
	img_height_width.reserve(16000);

	std::ifstream ifs(sDataRootDir + sImgNameList, std::ios::in);
	if (!ifs.is_open()) {
		std::cout << "open file fail: " << sDataRootDir + sImgNameList << std::endl;
		return EXIT_FAILURE;
	}

	std::string img_name_tmp;
	int h, w;
	while (ifs >> img_name_tmp) {
		ifs >> h >> w;
		img_height_width.push_back(h);
		img_height_width.push_back(w);
		img_names.push_back("image/" + img_name_tmp);
	}
	std::cout << "load image name: " << img_names.size() << std::endl;
	std::cout << "load image size: " << img_height_width.size() << std::endl;
	ifs.close();

	// read bundler data
	int num_camera, num_point;
	std::ifstream instream(sDataRootDir + sBundlerOutFile, std::ios::in);
	if (!instream.is_open()) {
		std::cout << "open bundler fail: " << sDataRootDir + sBundlerOutFile << std::endl;
		return EXIT_FAILURE;
	}

	std::string line_in_file;
	getline(instream, line_in_file);//header
	instream >> num_camera >> num_point;

	if (img_names.size() != num_camera) {
		std::cout << "num_camera error: " << num_camera << std::endl;
		return EXIT_FAILURE;
	}

	// read camera parameter
	for (int i = 0; i < num_camera; ++i) {
		double h = img_height_width[2 * i];
		double w = img_height_width[2 * i + 1];
		std::shared_ptr<View> ptr_view = std::make_shared<View>();
		ptr_view->id_intrinsic = i;
		ptr_view->id_pose = i;
		ptr_view->id_view = i;
		ptr_view->s_Img_path = img_names[i];
		ptr_view->ui_height = h;
		ptr_view->ui_width = w;
		sfm_data.views[i] = ptr_view;

		geometry::Pose3 pose;

		double f, k1, k2;
		instream >> f >> k1 >> k2;

		instream >> pose.rotation_(0, 0)
			>> pose.rotation_(0, 1)
			>> pose.rotation_(0, 2)
			>> pose.rotation_(1, 0)
			>> pose.rotation_(1, 1)
			>> pose.rotation_(1, 2)
			>> pose.rotation_(2, 0)
			>> pose.rotation_(2, 1)
			>> pose.rotation_(2, 2);

		Vec3 translation;
		instream >> translation[0] >> translation[1] >> translation[2];
		pose.center_ = -(pose.rotation().transpose() * translation);

		sfm_data.poses[i] = pose;

		using namespace openMVG::cameras;
		std::shared_ptr<cameras::IntrinsicBase> ptr_intrinsic = std::make_shared<Pinhole_Intrinsic_Radial_K3>
			(w, h, f, w / 2.0, h / 2.0, 0.0, 0.0, 0.0);  // setup no distortion as initial guess

		sfm_data.intrinsics[i] = ptr_intrinsic;
	}
	instream.close();

	std::cout << "views size: " << sfm_data.views.size() << std::endl;
	std::cout << "intrinsics size: " << sfm_data.intrinsics.size() << std::endl;
	std::cout << "extrinsics size: " << sfm_data.poses.size() << std::endl;


	// save to json format
	Save(sfm_data, stlplus::create_filespec(sOutDir, "sfm_data_converted", ".json"), ESfM_Data(ALL));
	//return 0;


	return EXIT_SUCCESS;
}
